from qr_loc.qrLoc import QrLoc
import math


class EasyPnp:
    def __init__(self, qr_cm):
        self.q = QrLoc()
        # 弧度
        self.theta = self.cal_theta()
        self.L = qr_cm
        # 左右边沿z坐标之差比上二维码边长L，就是sinβ，其中β就是二维码的法向量与Z轴的夹角。如果规定二维码偏左（如图中这样）时β为负，否则为正，那么有
        self.direction = None
        # 二维码中心点的z坐标,z为镜头方向
        self.z_center_cm = 0
        # self.beta = 0
        # self.left = None
        
        # 二维码的中心所在铅垂线与原点构成的平面和Z轴形成的夹角
        self.alpha = 0
    
    def cal_theta(self):
        h = 14.5
        dis = 21 + 29.7 + 4.89
        theta = 2 * math.atan((h / 2) / dis)
        
        print("theta=====", theta * 180 / 3.14)
        return theta
    
    def two_p_dis(self, p1, p2):
        dx = (p1[0] - p2[0]) ** 2
        dy = (p1[1] - p2[1]) ** 2
        return math.sqrt(dx + dy)
    
    def qr_process(self, img):
        dic = self.q.qr_loc(img)
        print("像素坐标", dic)
        h = dic["h"]
        w = dic['w']
        if not dic:
            return
        l_left_pixel = self.two_p_dis(dic["p1"], dic["p4"])
        print("左像素", l_left_pixel)
        z_left_cm = (self.L * h) / (l_left_pixel * 2 * math.tan(self.theta / 2))
        print("z左", z_left_cm)
        
        l_right_pixel = self.two_p_dis(dic["p2"], dic["p4"])
        print("右像素", l_right_pixel)
        z_right_cm = (self.L * h) / (l_right_pixel * 2 * math.tan(self.theta / 2))
        print("z右边", z_right_cm)
        
        self.z_center_cm = (z_left_cm + z_right_cm) / 2
        print("z中心=========", self.z_center_cm)
        
        # v = (z_left_cm - z_right_cm) / self.L
        # print ("beta取值",v)
        # self.left = True if v < 0 else False
        # print ("方向左边",self.left)
        # if abs(v) <= 1:
        #     # β就是二维码的法向量与Z轴的夹角。如果规定二维码偏左（如图中这样）时β为负，否则为正，
        #     self.beta = math.asin(_v)
        # print ("beta角度",self.beta*180/3.14)
        
        # 二维码的中心所在铅垂线与原点构成的平面和Z轴形成的夹角
        center_x_pixel = self.center_x_in_pixel(dic['p1'], dic['p2'], dic['p3'], dic['p4'])
        print("中心像素x", center_x_pixel)
        center_v = (center_x_pixel - w / 2) / (w / 2)
        print("center v", center_v)
        tan_t = math.tan(self.theta / 2) * w / h
        print ("tan_t",tan_t)
        self.alpha = math.atan(center_v * tan_t)
        print("alpha角度======", self.alpha * 180 / 3.14)
    
    def center_x_in_pixel(self, p1, p2, p3, p4):
        x1 = (p1[0] + p3[0]) / 2
        x2 = (p2[0] + p4[0]) / 2
        return (x1 + x2) / 2


if __name__ == '__main__':
    q = EasyPnp(5.45)
    q.qr_process("./img/4.jpg")
